% surface = getSurface(normals, grayMask)
%
% input:
%   * normals - A numRows-by-numColumns-by-3 matrix representing the
%   normals of each pixel in a numRows-by-numColumns image. normals(i,j,:)
%   is the normal vector of the scene point corresponding to pixel(i,j) in
%   the image. The normal vector is in the form of (n_x, n_y, n_z), where
%   the x axis is along the column index increasing direction (towards
%   right) and the y axis is along the row index increasing direction
%   (downwards). The z axis points out of the image.
%   * grayMask - A numRows-by-numColumns matrix masking the scene related
%   pixels. However, this matrix doesn't consist of only 0 and 1 indicating
%   exactly whether a pixel is or is not a scene point. The values kind of
%   range from 0 to 255. In this method, if grayMask(i, j) is larger than
%   127.5, then pixel(i,j) in the image is considered to be a scene point;
%   otherwise, the background pixel.
%
% output:
%   * surface - A numRows-by-numColumns matrix representing the z
%   coordinates of all pixels in the image. For a non-scene point pixel, z
%   = 0. For a scene point pixel, the closer the scene point is to the
%   camera, the larger the z value is.
%
% description:
% This method takes the normals of all (scene) pixels in the image and a
% (gray) mask to identify scene pixels to calculate the z coordinates of
% all scene pixels.
% The method is based on the heuristic that the normal vector of pixel(i,j)
% is perpendicular to the vector (i,j,z_ij)->(i+1,j,z_i+1j) and the vector
% (i,j,z_ij)->(i,j+1,z_ij+1).


function surface = getSurface(normals, grayMask)

[nRs, nCs, ~] = size(normals);
surface = zeros(nRs, nCs);


count2ij_map = zeros(0, 2);
ij2count_map = zeros(nRs,nCs);
count = 0;
threshold = max(max(grayMask))/2;
for i = 1:nRs
    for j = 1:nCs
        if grayMask(i,j) >= threshold
            count = count + 1;
            count2ij_map(count,:) = [i,j];
            ij2count_map(i,j) = count;
        end
    end
end

M = sparse(0,count);
v = sparse(0,1);
row = 0;
for p = 1:count
    i = count2ij_map(p,1);
    j = count2ij_map(p,2);
    n = reshape(normals(i,j,:),1,3);
    nx = n(1);
    ny = n(2);
    nz = n(3);
    if i+1 <= nRs
        if grayMask(i+1,j) >= threshold
            row = row + 1;
            M(row, ij2count_map(i+1,j)) = nz;
            M(row, p) = -nz;
            v(row,1) = -ny;
        end
    end
    
    if j+1 <= nCs
        if grayMask(i, j+1) >= threshold
            row = row + 1;
            M(row, ij2count_map(i,j+1)) = nz;
            M(row, p) = - nz;
            v(row, 1) = -nx;
        end
    end
end

z = M\v;
z = z-min(z);
for p = 1:count
    index = count2ij_map(p,:);
    i = index(1);
    j = index(2);
    surface(i, j) = z(p);
end


end

